function [ pd ] = undistort( p, K, D )
%UNDISTORT Undistort a homogenous pixel co-ordinate
%   Given a homogenous pixel with co-ordinates p = [x y w] (or [u v 1]),
% an intrinsics matrix K = [fx gamma cx; 0 fy cy; 0 0 1] and distortion 
% coefficients D = [k1 k2 p1 p2 k3], returns the undistorted pixel 
% co-ordinates pd = [u' v' 1]

% Copyright (c) Michael Warren 2014

% This file is part of the AMMCC Toolbox.

% The AMMCC Toolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% The AMMCC Toolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Lesser General Public License
% along with the AMMCC Toolbox.  If not, see <http://www.gnu.org/licenses/>.

    while(size(D,2) < 8)
        D = [D 0];
    end

    iters = 10;
    
    pp = inv(K)*p;
    pp = pp./pp(3);

    x = pp(1);
    y = pp(2);
    x0 = x;
    y0 = y;
    for j=0:1:iters
        r2 = x*x + y*y;
        ic1 = (1 + ((D(8)*r2 + D(7))*r2 + D(6))*r2);
        ic2 = (1 + ((D(5)*r2 + D(2))*r2 + D(1))*r2);
        icdist = ic1/ic2;
        deltaX = 2*D(3)*x*y + D(4)*(r2 + 2*x*x);
        deltaY = D(3)*(r2 + 2*y*y) + 2*D(4)*x*y;
        x = (x0 - deltaX)*icdist;
        y = (y0 - deltaY)*icdist;
    end

    pd = K*[x y 1]';
    pd = pd./pd(3);
end

